/*========================================================================
			ROBOCON 2008 - PUNE INDIA
			BKIT4 - CNTT DH BACH KHOA TPHCM
			HARDWARE CLUB - AUTO ROBOT2
			File : forward_encoder.c
=========================================================================*/
#include "Robocon.h"
int pulse_a1,pulse_b1;
//chay do line va theo mot khoang cach dua tron length_encoder_forward
//----------------------------------------------------------------
void forward_encoder()								
{
	switch (function_encoder)
	{
		case FORWARD_ENCODER : //khoi dong chay mot thoi gian timer1
			test_forward_pd();
			AD_to_led(quang_duong);
			if (quang_duong >= length_encoder_forward)
				function_encoder = FORWARD_ENCODER_END;
			break;
		case FORWARD_ENCODER_END : //chay thoi gian do forward3 dua ra
			timer1(250);//neu khong can phanh thi co the khong dungn timer1
			Vel_L = 0;
			Vel_R = 0;
			bool_hold_vel = 1;//cho phep 
			function_encoder = FORWARD_ENCODER_END1;
			AD_to_led(quang_duong);
			break;
		case FORWARD_ENCODER_END1://chay nhanh vua va test co gap line ngang dau tien hay khong
			AD_to_led(quang_duong);
			if(timer_flag1)
			{
				bool_hold_vel = 0;
				finish_encoder = 1;
				function_encoder = NO_THING;
				//init_encoder(0);
			}
			break;
		default :
			break;
	}
}
//----------------------------------------------------------------
void forward_encoder_slow()								
{
	switch (function_encoder)
	{
		case FORWARD_ENCODER_SLOW:
			timer1(2000);
			test_forward_pd();
			AD_to_led(quang_duong);
			function_encoder = FORWARD_ENCODER_SLOW1;
			break;
		case FORWARD_ENCODER_SLOW1 : //khoi dong chay mot thoi gian timer1
			
			test_forward_pd();			
			AD_to_led(quang_duong);
			pulse_a1 = quang_duong;
			if (quang_duong >= length_encoder_forward)
			{
				Reset_hold_vel();
				speed(0,0);
				finish_encoder = 1;
				function_encoder = NO_THING;
				break;
			}
			if(timer_flag1)
			{
				if(pulse_a1 > pulse_b1)
				{
					function_encoder = FORWARD_ENCODER_SLOW;
					pulse_b1 = pulse_a1;
				}
				else
				{
					function_encoder = FORWARD_ENCODER_SLOW_END;
					pulse_b1 = 0;
					pulse_a1 = 0;
				}	
			}break;
		case FORWARD_ENCODER_SLOW_END : //chay thoi gian do forward3 dua ra
			AD_to_led(quang_duong);
			if(timer_flag1)
			{
				Reset_hold_vel();
				speed(0,0);
				finish_encoder = 1;
				function_encoder = NO_THING;
			}
			break;
		case FORWARD_ENCODER_SLOW_END1://chay nhanh vua va test co gap line ngang dau tien hay khong
			
			break;
		default :
			break;
	}
}
//----------------------------------------------------------------
void forward_encoder_blind()								
{
	switch (function_encoder)
	{
		case FORWARD_ENCODER_BLIND : //khoi dong chay mot thoi gian timer1	
			AD_to_led(quang_duong);
			if (quang_duong>=length_encoder_forward)
				function_encoder = FORWARD_ENCODER_BLIND_END;
			break;
		case FORWARD_ENCODER_BLIND_END : //chay thoi gian do forward3 dua ra
			timer1(200);//neu khong can phanh thi co the khong dungn timer1
			Vel_L = 0;
			Vel_R = 0;
			function_encoder = FORWARD_ENCODER_BLIND_END1;
			AD_to_led(quang_duong);
			break;
		case FORWARD_ENCODER_BLIND_END1://chay nhanh vua va test co gap line ngang dau tien hay khong
			AD_to_led(quang_duong);
			if(timer_flag1)
			{
				bool_hold_vel = 0;//khong giu van toc nua
				finish_encoder = 1;
				function_encoder = NO_THING;
			}
			break;
		default :
			break;
	}
}
//----------------------------------------------------------------